﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Dispatch.DispatchData;
using Dispatch.Model;
using Dispatch.Receive;
using Dispatch.Utils;

namespace Dispatch.Receive
{
    /// <summary>
    /// AGV指令处理，因为代码量很大，所以单独拿了出来 
    /// </summary>
    internal class DispatchAgv
    {
     

        /// <summary>
        /// 处理指令
        /// </summary>
        /// <param name="receiveData"></param>
        internal async void Dispatch(AgvReceiveData receiveData)
        {
            DeviceModel? deviceModel = null;
            IDeviceEvent? deviceEvent = null;
            MarkerModel? marker = MarkerDataSet.GetMarkerByPos(receiveData.X, receiveData.Y);
            if (marker != null)
            {
                long DeviceID = DeviceMountSet.GetDeviceByMarker(marker.MarkerID);
                deviceModel = DeviceMountSet.GetDevice(DeviceID);
                if (deviceModel != null)
                {
                    deviceEvent = DeviceFactory.GetDevice(deviceModel.DeviceType);
                }
            }

            //随时都更新位置
            await AgvDataSet.UpdateAgvAsync(receiveData);


            byte[] buf;
            switch (receiveData.State)
            {
                case AgvStateEnum.行驶: 
                    _ = CheckRunAsync(receiveData);
                    break;
                case AgvStateEnum.停止:
                    //什么都不用做
                    break;
                case AgvStateEnum.运动控制:
                    _ = CheckControlAsync(receiveData);
                    break;
                case AgvStateEnum.到达:
                     CheckStop(receiveData, marker, deviceModel);
                    break;
                case AgvStateEnum.异常:
                    //什么都不用做
                    break;
                case AgvStateEnum.上层传出中:
                    //看情况处理
                    break;
                case AgvStateEnum.下层传出中:
                    //看情况处理
                    break;
                case AgvStateEnum.上层传入中:
                    //看情况处理
                    break;
                case AgvStateEnum.下层传入中:
                    //看情况处理
                    break;
                case AgvStateEnum.上层传出完成:
                    //向传输坞发送数据
                    if (deviceModel != null && deviceEvent != null)
                    {
                        buf = deviceEvent.AgvTransOk(deviceModel.DockNum, true, false);
                        _ = Tools.SendDataToDeviceAsync(deviceModel.SendPort, deviceModel.IP, deviceModel.Port, buf);
                    }
                    break;
                case AgvStateEnum.下层传出完成:
                    if (deviceModel != null && deviceEvent != null)
                    {
                        buf = deviceEvent.AgvTransOk(deviceModel.DockNum, false, true);
                        _ = Tools.SendDataToDeviceAsync(deviceModel.SendPort, deviceModel.IP, deviceModel.Port, buf);
                    }
                    break;
                case AgvStateEnum.上层传入完成:
                    if (deviceModel != null && deviceEvent != null)
                    {
                        buf = deviceEvent.AgvTransOk(deviceModel.DockNum, true, false);
                        _ = Tools.SendDataToDeviceAsync(deviceModel.SendPort, deviceModel.IP, deviceModel.Port, buf);
                    }
                    break;
                case AgvStateEnum.下层传入完成:
                    if (deviceModel != null && deviceEvent != null)
                    {
                        buf = deviceEvent.AgvTransOk(deviceModel.DockNum, false, true);
                        _ = Tools.SendDataToDeviceAsync(deviceModel.SendPort, deviceModel.IP, deviceModel.Port, buf);
                    }
                    break;

                case AgvStateEnum.上入下出中:
                    //看情况处理
                    break;
                case AgvStateEnum.上出下入中:
                    //看情况处理
                    break;

                case AgvStateEnum.上入下出完成:
                    if (deviceModel != null && deviceEvent != null)
                    {
                        buf = deviceEvent.AgvTransOk(deviceModel.DockNum, true, true);
                        _ = Tools.SendDataToDeviceAsync(deviceModel.SendPort, deviceModel.IP, deviceModel.Port, buf);
                    }
                    break;
                case AgvStateEnum.上出下入完成:
                    if (deviceModel != null && deviceEvent != null)
                    {
                        buf = deviceEvent.AgvTransOk(deviceModel.DockNum, true, true);
                        _ = Tools.SendDataToDeviceAsync(deviceModel.SendPort, deviceModel.IP, deviceModel.Port, buf);
                    }
                    break;
                case AgvStateEnum.可传输:
                    //发送到达
                    if (deviceModel != null && deviceEvent != null)
                    {
                        buf = deviceEvent.AgvIsArrived(deviceModel.DockNum);
                        _ = Tools.SendDataToDeviceAsync(deviceModel.SendPort, deviceModel.IP, deviceModel.Port, buf);
                    }
                    break;
            }
        }


        /// <summary>
        /// 运行中的判断 
        /// </summary>
        /// <param name="receiveData"></param>
        /// <returns></returns>
        async Task CheckRunAsync(AgvReceiveData receiveData)
        {
            #region 区域核对
            WaringAreaState state=await WarningAreaDataSet.CheckArea(receiveData.AgvID, receiveData.X, receiveData.Y);
            if (state.AreaID == "")
            {
                //离开了区域
                string AreaID= await WarningAreaDataSet.GetAreaIDOfAgv(receiveData.AgvID);
                if (AreaID != "")
                {
                    //释放
                    _=WarningAreaDataSet.ReleaseAsync(AreaID); 
                }
            }
            else
            {
                //进入了区域
                if (state.Locked)
                {
                    //需要停下来
                    byte[] sbu = AgvEvent.Traffic(receiveData.AgvID, true);
                    _=AgvDataSet.SendToAgvAsync(receiveData.AgvID, sbu);
                }
                else
                {
                    //锁了区域，什么也不做
                }
            }
            #endregion


            #region 如果不在区域内，检测前方区域内是否有其它AGV，如果有，停下来
             
            WaringArea? area = GetVirtualArea(receiveData.AgvID);
            if (area == null) return;

            List<AgvData> agvList = await AgvDataSet.GetAgvListAsync(area.MinX, area.MinY, area.MaxX, area.MaxY, receiveData.AgvID);
            if (agvList.Count == 0) return; //前方区域没有其它AGV

            foreach (AgvData agv in agvList)
            {
                PointF targetPoint = new PointF(agv.X, agv.Y);
                if (WarningAreaDataSet.IsInPolygon(targetPoint, area.PointList))
                {
                    //在区域内，当前agv需要停下来
                    byte[] buf = AgvEvent.Traffic(receiveData.AgvID, true);
                    _ = AgvDataSet.SendToAgvAsync(receiveData.AgvID, buf);
                    break;
                }
            } 
            #endregion 
        }

        /// <summary>
        /// 判断前方区域是否存在其它agv,如果没有，启动
        /// </summary>
        /// <param name="receiveData"></param>
        /// <returns></returns>
        async Task CheckControlAsync(AgvReceiveData receiveData)
        {
            string AreaID = await WarningAreaDataSet.GetAreaIDOfAgv(receiveData.AgvID);
            if (AreaID != "")
            {
                //不处理区域范围内
                return; 
            }
             
            WaringArea? area=GetVirtualArea(receiveData.AgvID);
            if (area == null)
            {
                //区域无效，让agv跑起来
                byte[] buf = AgvEvent.Traffic(receiveData.AgvID, false);
                _ = AgvDataSet.SendToAgvAsync(receiveData.AgvID, buf);
                return;
            };

            List<AgvData> agvList = await AgvDataSet.GetAgvListAsync(area.MinX, area.MinY, area.MaxX, area.MaxY, receiveData.AgvID);
            if (agvList.Count == 0)
            {
                //前方区域没有其它AGV,让agv跑起来
                byte[] buf = AgvEvent.Traffic(receiveData.AgvID, false);
                _ = AgvDataSet.SendToAgvAsync(receiveData.AgvID, buf);
                return; 
            }
            bool hasAgv = false;
            foreach(AgvData agv in agvList)
            {
                PointF targetPoint = new PointF(agv.X, agv.Y);
                if (WarningAreaDataSet.IsInPolygon(targetPoint, area.PointList))
                {
                    hasAgv = true;
                    break;
                }
            }
            if (hasAgv == false)
            {
                //前方无车，让agv跑起来
                byte[] buf = AgvEvent.Traffic(receiveData.AgvID, true);
                _ = AgvDataSet.SendToAgvAsync(receiveData.AgvID, buf);
            }
        }


        /// <summary>
        /// AGV到达的判断
        /// </summary>
        /// <param name="receiveData"></param>
        /// <param name="marker"></param>
        /// <param name="deviceModel"></param>
        void  CheckStop(AgvReceiveData receiveData ,MarkerModel? marker, DeviceModel? deviceModel)
        {
           if(marker == null)
            {
                //没有地标，什么也不用做
                return ;
            }
           MarkerEnum markerEnum=(MarkerEnum)marker.Type;
            switch (markerEnum)
            {
                case MarkerEnum.待命:
                    //什么也不用做
                    break;
                case MarkerEnum.传输坞:
                    //允许传输了
                    if (deviceModel != null)
                    {
                        //绑定传输坞，方便快速查找
                        //一台AGV只能出现在一个传输坞前
                        DeviceMountSet.BindAgvAsync(deviceModel.DeviceID, receiveData.AgvID);
                    } 
                    break;
                case MarkerEnum.充电:
                    //什么也不用做
                    break;
                case MarkerEnum.传输坞选择:
                    //选传输坞
                    _ = ChooseDeviceAsync(receiveData, marker);
                    break;
                case MarkerEnum.坞前:
                    //可以考虑优化，即不满足某些条件，AGV不能移动。视情况而定
                    //注意，具体情况不同，工序也不同，处理策略也不同，此处代码量很奇多
                    //根据以往的项目实施经验，AGV停在此处分为没有完成传输和已经完全传输，光完成传输后去往何处，需要按工序和现场规划进行确定

                    //例：传输完成，解锁对应的传输坞,然后让AGV走指定的路由
                    string TransMarker = MarkerDataSet.GetTransMarker(marker.MarkerID);//取传输坞
                    long DeviceID= DeviceMountSet.GetDeviceByMarker(TransMarker); 
                    DeviceMountSet.BindAgvAsync(DeviceID, 0);

                    int RouteID = RouteDataSet.GetRoute(marker.MarkerID);
                    if (RouteID > 0)
                    {
                        byte[] buf = AgvEvent.GoRoute(receiveData.AgvID, RouteID);
                        _ =AgvDataSet.SendToAgvAsync(receiveData.AgvID, buf);
                    }                   
                    break;
            } 
        }


        /// <summary>
        /// 选择传输坞
        /// </summary>
        /// <param name="receiveData"></param>
        /// <param name="marker"></param>
        /// <returns></returns>
        async Task ChooseDeviceAsync(AgvReceiveData receiveData, MarkerModel marker)
        {

            DeviceModel? device = await DeviceMountSet.ChooseDevice(marker.Process);
            if (device == null)
            {
                //没有合适的,找停泊位
                StayMarker? stay = await MarkerDataSet.GetEmptyStayMarker(marker.Process);
                if(stay != null)
                {
                    //存在待命点
                    //锁住
                     _= MarkerDataSet.LockStayMarker(stay.MarkerID, receiveData.AgvID);
                    //AGV移动过去
                    int RouteID = RouteDataSet.GetRoute(marker.MarkerID, stay.MarkerID);
                    if (RouteID > 0)
                    {
                        byte[] buf = AgvEvent.GoRoute(receiveData.AgvID, RouteID);
                        _ = AgvDataSet.SendToAgvAsync(receiveData.AgvID, buf);
                    }
                }
                else
                {
                    //没有可用待命点，原地等
                    return;
                }
            }
            else
            {
                //传输坞前面的位置
                string TargetMarker = MarkerDataSet.GetBeforeMarker(device.MarkerID);
                int RouteID = RouteDataSet.GetRoute(marker.MarkerID, TargetMarker);
                if (RouteID > 0)
                {
                    byte[] buf = AgvEvent.GoRoute(receiveData.AgvID, RouteID);
                    _ = AgvDataSet.SendToAgvAsync(receiveData.AgvID, buf);
                }
            }
         
        }


        /// <summary>
        /// 生成前方的虚拟区域，数学计算，不需要Task化
        /// </summary>
        /// <param name="AgvID"></param>
        /// <returns></returns>
        WaringArea? GetVirtualArea(int AgvID)
        {
            AgvData? data= AgvDataSet.GetAgvData(AgvID);
            if (data == null) return null;
                        
            if(data.LastX==0&& data.LastY==0) return null; //过滤数据不准确的
 
            float sin = MathF.Sin(data.Radian);
            float cos = MathF.Cos(data.Radian);

            //计算前方中心点(双方半径+缓冲距离）
            float dist = Config.FrontDist + 2 * Config.AgvRadius;

            float tx =(float)(data.X  + dist * cos);
            float ty =(float)(data.Y  + dist * sin);
            float r = Config.AgvRadius;

            List<PointF> points= new List<PointF>();
            points.Add(new PointF(tx-r,ty-r));
            points.Add(new PointF(tx-r,ty+r));
            points.Add(new PointF(tx+r,ty+r));
            points.Add(new PointF(tx+r,ty-r));
            //点位旋转
            for(int i = 0; i < points.Count; i++)
            {
                var item=points[i];
                var rx = item.X * cos + item.Y * sin;
                var ry = item.X * sin - item.Y * cos;
                item.X = rx;
                item.Y = ry;
            }
            WaringArea area = new WaringArea();
            area.PointList= points;
            area.MaxX = (int) points.Max(x => x.X)+1;
            area.MinX = (int) points.Min(x => x.X)-1;
            area.MaxY = (int) points.Max(x => x.Y)+1;
            area.MinY = (int) points.Min(x => x.Y)-1; 
            return area;
        }
    }
}
